#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <memory.h>>
#include "iap/include/ymodem.h"

#include <QDebug>
#include <QString>
#include <QTime>
#include <QThread>

#define DEBUG_INFO  qDebug() << __FILE__ << __func__ << __LINE__;

#define BUFFER_SIZE (2048)

typedef struct {
    ReadIo_t     readIo;
    WriteIo_t    writeIo;
    ReadFlash_t  readFlash;
    WriteFlash_t writeFlash;
    int8_t       filename[FILE_NAME_LENGTH];
    int32_t      len;
    uint8_t      md5[16];
    uint8_t      pkg[1030];
    uint8_t      buf[BUFFER_SIZE];
} Ymodem_t;

uint16_t YCode_C      = 0x0043; // CRC16
uint16_t YCode_ACK_C  = 0x4306; // ACK CRC16
uint16_t YCode_ACK    = 0x0006; // ACK 
uint16_t YCode_NAK    = 0x0015; // NAK
uint16_t YCode_CA_CA  = 0x1818; // CA CA
uint16_t YCode_RES    = 0x5252; // resend self def

Ymodem_t __ymodem;

int32_t __StartPackage(char *filename, int32_t length);
int32_t __DataPackage(uint8_t *src, uint8_t cnt, int32_t dataBlkSize);
int32_t __EmptyPackage();
YmodemStatus_t __ReceivePkg();
YmodemStatus_t __ReceiveOnce();
YmodemStatus_t __TransmitOnce(char *filename, int32_t lenAll, uint8_t * md5);
uint16_t __crc16_xmodem(uint8_t *ptr, int len);
int32_t __IsTransmitTimeOut(uint16_t code);
uint16_t __RecvTransmitCode(int32_t step);

/* Public functions ---------------------------------------------------------*/

void Ymodem_Init(ReadIo_t readIo, WriteIo_t writeIo, ReadFlash_t readFlash, WriteFlash_t writeFlash)
{
    memset(&__ymodem, 0, sizeof(Ymodem_t));
    __ymodem.readIo     = readIo;
    __ymodem.writeIo    = writeIo;
    __ymodem.readFlash  = readFlash;
    __ymodem.writeFlash = writeFlash;
}

YmodemStatus_t Ymodem_Receive()
{
    YmodemStatus_t status;
    do {
        status = __ReceiveOnce();
    } while(status == Ymodem_Recving);
    return status;
}

YmodemStatus_t Ymodem_Transmit(char *filename, int32_t len, uint8_t * md5)
{
    qDebug() << __FILE__ << __LINE__ << __func__;
    YmodemStatus_t status;
    do {
        status = __TransmitOnce(filename, len, md5);
    } while (status == Ymodem_Sending);
    qDebug() << __FILE__ << __LINE__ << __func__;
    return status;
}

/* Private functions ---------------------------------------------------------*/

YmodemStatus_t __ReceivePkg()
{
    static int32_t maxlen = 0;
    static int32_t wi = 0;
    int32_t i = 0;
    uint16_t crc = 0;
    YmodemStatus_t retVal = Ymodem_DataRead;

    memset(__ymodem.buf, 0, 10);
    int32_t len = (__ymodem.readIo)(__ymodem.buf, BUFFER_SIZE);

    for (i = 0; i < len; ++i) {
        if (wi == 0 && (__ymodem.buf[i] == SOH || __ymodem.buf[i] == STX)) {
            __ymodem.pkg[0] = __ymodem.buf[i];
            wi = 1;
        } else if (wi == 1) {
            __ymodem.pkg[1] = __ymodem.buf[i];
            wi = 2;
        } else if (wi == 2) {
            __ymodem.pkg[2] = __ymodem.buf[i];
            if ( __ymodem.pkg[2] + __ymodem.pkg[1] == 0xFF) {
                wi = 3;
                maxlen = __ymodem.pkg[0] == SOH ? 133 : 1029;
            } else {
                wi = 0;
                maxlen = 0;
            }
        } else if (maxlen){
            if (wi < maxlen) {
                __ymodem.pkg[wi] = __ymodem.buf[i];
                ++wi;
            } 
            
            if (wi >= maxlen) {
                crc  = __ymodem.pkg[maxlen - 2] << 8;
                crc += __ymodem.pkg[maxlen - 1];// printf("maxlen %d\n", maxlen);
                uint16_t ckcrc = __crc16_xmodem(&(__ymodem.pkg[3]), maxlen - 5);// printf("%04X %04X\n", crc, ckcrc);
                if (crc == ckcrc) {
                    retVal = Ymodem_PkgRead;
                } else {
                    retVal = Ymodem_CrcErr;
                }
                wi = 0;
                maxlen = 0;
            }
        } else ;
    }
    if (len == 0) {
        retVal = Ymodem_NoDataReadYet;
    }
	return retVal;
}

int32_t __StartPackage(char *filename, int32_t length, uint8_t * md5)
{
    /* first 3 bytes are constant */
    uint8_t * pdata = __ymodem.pkg;

    pdata[0] = SOH;
    pdata[1] = 0x00;
    pdata[2] = 0xff;

    int32_t offset = 3;
    memset(pdata + 3, 0, 128);
    /* Filename written */
    strncpy(pdata + offset, filename, FILE_NAME_LENGTH);
    int32_t fnlen = strlen(filename);
    if (fnlen >= FILE_NAME_LENGTH) {
        pdata[FILE_NAME_LENGTH + 3] = '\0';
        fnlen = FILE_NAME_LENGTH;
    }
    offset += fnlen + 1;
    /* file size written */
    offset += sprintf((char *)(pdata + offset), "%d", length) + 1;
    /* add md5 */
    memcpy(pdata + offset, md5, 16);

    /* add crc16 */
    uint16_t ckcrc = __crc16_xmodem(pdata + 3, 128);
    pdata[131] = (ckcrc >> 8) & 0x00FF;
    pdata[132] = ckcrc & 0x00FF;

    return 133;
}

int32_t __DataPackage(uint8_t *src, uint8_t cnt, int32_t dataBlkSize)
{
    if (dataBlkSize > 1024) {
        dataBlkSize = 1024;
    }
    int32_t len = 0;
    uint8_t * pdata = __ymodem.pkg;
    pdata[0] = dataBlkSize > 128 ? STX : SOH;
    if (pdata[0] == STX) len = 1029; else
    if (pdata[0] == SOH) len =  133; else;
    pdata[1] = cnt;
    pdata[2] = ~cnt;
    memcpy(pdata + 3, src, dataBlkSize);
    int32_t lenNull = dataBlkSize > 128 ? 1024 - dataBlkSize : 128 - dataBlkSize;
    if (lenNull > 0) {
        memset(pdata + 3 + dataBlkSize, 0x1A, lenNull);
    }

    uint16_t ckcrc = __crc16_xmodem(pdata + 3, len - 5);
    pdata[len - 2] = (ckcrc >> 8) & 0x00FF;
    pdata[len - 1] = ckcrc & 0x00FF;

    return len;
}

int32_t __EmptyPackage()
{
    uint8_t * pdata = __ymodem.pkg;
    pdata[0] = SOH;
    pdata[1] = 0;
    pdata[2] = 0xFF;
    memset(pdata + 3, 0, 130);
    return 133;
}

YmodemStatus_t __ReceiveOnce()
{
    static uint8_t cntPkg   = 0;
    static int32_t step     = 0;
    static int32_t leftToWrite = 0;
    static uint8_t sentData[2] = {0, 0};
    static QTime to;

    YmodemStatus_t retVal = Ymodem_Recving;

    if (step == 0) {
        (__ymodem.writeIo)((uint8_t *)&YCode_C, 1);
        QThread::msleep(500);
    } 

    YmodemStatus_t recvStatus = __ReceivePkg();

    if (step == 0 && recvStatus == Ymodem_PkgRead) {
        if (cntPkg == 0 && __ymodem.pkg[1]  == 0 && __ymodem.pkg[3]) {
            strncpy(__ymodem.filename, &(__ymodem.pkg[3]), FILE_NAME_LENGTH);
            __ymodem.len = atoi(__ymodem.pkg + strlen(&(__ymodem.pkg[3])) + 4);
            leftToWrite = __ymodem.len;
            sentData[0] = ACK;
            sentData[1] = CRC16;
            (__ymodem.writeIo)((uint8_t *)&YCode_ACK, 1);
            (__ymodem.writeIo)((uint8_t *)&YCode_C, 1);
            step = 1;
            cntPkg = 1;
        } else {
            retVal = Ymodem_Resend;
        }
    } else if (step == 1 && recvStatus == Ymodem_PkgRead && cntPkg == __ymodem.pkg[1]) {
        int32_t lenWF = __ymodem.pkg[0] == STX ? 1024 : 128;
        if (leftToWrite < lenWF) {
            lenWF = leftToWrite;
        }
        if ((__ymodem.writeFlash)(__ymodem.pkg + 3, lenWF) > 0) {
            leftToWrite -= lenWF;
            sentData[0] = ACK;
            sentData[1] = 0;
            (__ymodem.writeIo)((uint8_t *)&YCode_ACK, 1);
            ++cntPkg;
        } else {
            retVal = Ymodem_Resend;
        }
    } else if (step == 1 && recvStatus == Ymodem_DataRead) {
        if (__ymodem.buf[0] == EOT) {
            sentData[0] = NAK;
            sentData[1] = 0;
            (__ymodem.writeIo)((uint8_t *)&YCode_NAK, 1);
            step = 2;
        } else {
            retVal = Ymodem_Resend;
        }
    } else if (step == 2 && recvStatus == Ymodem_DataRead) {
        if (__ymodem.buf[0] == EOT) {
            sentData[0] = ACK;
            sentData[1] = CRC16;
            (__ymodem.writeIo)((uint8_t *)&YCode_ACK, 1);
            (__ymodem.writeIo)((uint8_t *)&YCode_C, 1);
            step = 3;
        } else {
            retVal = Ymodem_Resend;
        }
    } else if (step == 3 && recvStatus == Ymodem_PkgRead) {
        if (__ymodem.pkg[131] == 0 && __ymodem.pkg[132] == 0) {
            (__ymodem.writeIo)((uint8_t *)&YCode_ACK, 1);
            retVal = Ymodem_RecvDone;
        } else {
            retVal = Ymodem_Resend;
        }
    } else ;


    if (recvStatus == Ymodem_CrcErr || recvStatus == Ymodem_NoDataReadYet) {
        retVal = Ymodem_Resend;
    }

    if (retVal == Ymodem_Recving) {
        to = QTime::currentTime().addMSecs(3000);
    }

    if (retVal == Ymodem_Resend) {
        if (recvStatus == Ymodem_NoDataReadYet) {
            if (sentData[0]) (__ymodem.writeIo)(sentData, 1);
            if (sentData[1]) (__ymodem.writeIo)(sentData + 1, 1);
        } else {
            (__ymodem.writeIo)((uint8_t*)&YCode_RES, 2);
        }
        if (QTime::currentTime() < to) {
            retVal = Ymodem_Recving;
        } else {
            retVal = Ymodem_Aborted;
        }
    }

    if (recvStatus == Ymodem_DataRead && __ymodem.buf[0] == CA && __ymodem.buf[1] == CA) {
        retVal = Ymodem_Aborted;
    }

    if (retVal != Ymodem_Recving) {
        cntPkg = 0;
        step = 0;
        leftToWrite = 0;
        sentData[0] = 0;
        sentData[1] = 0;
    }

    return retVal;
}

YmodemStatus_t __TransmitOnce(char *filename, int32_t lenAll, uint8_t * md5)
{
    static int32_t step = 0;
    static uint8_t cntDP = 0;
    static int32_t wlen = 0;

    YmodemStatus_t retVal = Ymodem_Sending;
    uint16_t recvCode = __RecvTransmitCode(step);
    if (__IsTransmitTimeOut(recvCode)) {
        recvCode = YCode_CA_CA;
    }

    if (step == 0 && recvCode == YCode_C) {
        wlen = __StartPackage(filename, lenAll, md5);
        (__ymodem.writeIo)(__ymodem.pkg, wlen);
        step = 1;
    } else if (step == 1 && recvCode == YCode_ACK_C) {
        cntDP = 1;
        int32_t lenRF = (__ymodem.readFlash)(__ymodem.buf, 1024);
        wlen = __DataPackage(__ymodem.buf, cntDP, lenRF);
        (__ymodem.writeIo)(__ymodem.pkg, wlen);
        step = 2;
    } else if (step == 2 && recvCode == YCode_ACK) {
        ++cntDP;
        int32_t lenRF = (__ymodem.readFlash)(__ymodem.buf, 1024);
        if (lenRF > 0) {
            wlen = __DataPackage(__ymodem.buf, cntDP, lenRF);
            (__ymodem.writeIo)(__ymodem.pkg, wlen);
        } else {
            wlen = 1;
            memset(__ymodem.pkg, 0, wlen);
            __ymodem.pkg[0] = EOT;
            (__ymodem.writeIo)(__ymodem.pkg, wlen);
            step = 3;
        }
    } else if (step == 3 && recvCode == YCode_NAK) {
        wlen = 1;
        memset(__ymodem.pkg, 0, wlen);
        __ymodem.pkg[0] = EOT;
        (__ymodem.writeIo)(__ymodem.pkg, wlen);
        step = 4;
    } else if (step == 4 && recvCode == YCode_ACK_C) {
        wlen = __EmptyPackage();
        (__ymodem.writeIo)(__ymodem.pkg, wlen);
        step = 5;
    } else if (step == 5 && recvCode == YCode_ACK) {
        step = 6;
        retVal = Ymodem_SendDone;
    } else if (recvCode == YCode_CA_CA) {
        retVal = Ymodem_Aborted;
    } else if (recvCode == YCode_RES) {
        (__ymodem.writeIo)(__ymodem.pkg, wlen);
    } else ;

    if (retVal != Ymodem_Sending) {
        step = 0;
        cntDP = 0;
        wlen = 0;
        __RecvTransmitCode(6);
        __IsTransmitTimeOut(0);
    }
    return retVal;
}

uint16_t __crc16_xmodem(uint8_t *ptr, int len)
{
    int i;
    uint16_t crc = 0x0000;
    
    while(len--) {
        crc ^= (uint16_t)(*ptr++) << 8;
        for (i = 0; i < 8; ++i) {
            if (crc & 0x8000)
                crc = (crc << 1) ^ 0x1021;
            else
                crc <<= 1;
        }
    }
    
    return crc;
}

uint16_t __RecvTransmitCode(int32_t step)
{
    union  {
        uint16_t code;
        uint8_t  rCdata[2];
    } recvCode;

    recvCode.code = 0;
    if  (step != 6) {
        int32_t len = (__ymodem.readIo)(__ymodem.buf, BUFFER_SIZE);
        if (len == 1) {
            if (step == 1 || step == 4 || __ymodem.buf[0] == CA) {
                recvCode.rCdata[0] = __ymodem.buf[0];
                __ymodem.buf[0] = 0;
                len = (__ymodem.readIo)(__ymodem.buf, BUFFER_SIZE);
                recvCode.rCdata[1] = __ymodem.buf[0];
            } else {
                recvCode.rCdata[0] = __ymodem.buf[0];
            }
        } else if (len > 1) {
            recvCode.rCdata[0] = __ymodem.buf[0];
            recvCode.rCdata[1] = __ymodem.buf[1];
        } else {
            QThread::msleep(5);
        }
    }

    return recvCode.code;
}

int32_t __IsTransmitTimeOut(uint16_t code)
{
    static QTime to;
    static int32_t resCnt = 0;
    int32_t retVal = 0;
    if (code == YCode_C     ||
        code == YCode_ACK_C ||
        code == YCode_ACK   ||
        code == YCode_NAK   ||
        code == YCode_CA_CA ||
        code == YCode_RES)
    {
        if (code == YCode_RES) {
            ++resCnt;
        } else {
            to = QTime::currentTime().addMSecs(3000);
            resCnt = 0;
        }
    }
    if (to.isNull()) {
        to = QTime::currentTime().addMSecs(120000);
    }
    if (QTime::currentTime() > to || resCnt > 10) {
        retVal = 1;
    }
    if (code == 0) {
        to = QTime();
    }
    return retVal;
}

/* EOF */
